#include <libge/libge.h>
#include "Location.h"

Location::Location(){
	X = Y = Z = 0.0;
	Yaw = Pitch = HeadYaw = 0.0;

	mode = LOCATION_MODE_DIRECT;
	targetX = targetY = targetZ = 0.0;
	targetYaw = targetPitch = targetHeadYaw = 0.0;
	t = ((float)geGetTick()) / 1000.0;
	dt = 0.0;
}

Location::Location(const Location& loc){
	X = loc.X;
	Y = loc.Y;
	Z = loc.Z;
	Yaw = loc.Yaw;
	Pitch = loc.Pitch;
	HeadYaw = loc.HeadYaw;
	
	mode = loc.mode;
	targetX = loc.targetX;
	targetY = loc.targetY;
	targetZ = loc.targetZ;
	targetYaw = loc.targetYaw;
	targetPitch = loc.targetPitch;
	targetHeadYaw = loc.targetHeadYaw;
	t = loc.t;
	dt = 0.0;
}

Location::Location(double x, double y, double z){
	X = x;
	Y = y;
	Z = z;
	Yaw = Pitch = HeadYaw = 0.0;
	
	mode = LOCATION_MODE_DIRECT;
	targetX = x;
	targetY = y;
	targetZ = z;
	targetYaw = targetPitch = targetHeadYaw = 0.0;
	t = ((float)geGetTick()) / 1000.0;
	dt = 0.0;
}

Location::Location(double x, double y, double z, double yaw, double pitch){
	X = x;
	Y = y;
	Z = z;
	Yaw = yaw;
	Pitch = pitch;
	HeadYaw = 0.0;
	
	mode = LOCATION_MODE_DIRECT;
	targetX = x;
	targetY = y;
	targetZ = z;
	targetYaw = yaw;
	targetPitch = pitch;
	targetHeadYaw = 0.0;
	t = ((float)geGetTick()) / 1000.0;
	dt = 0.0;
}

Location::Location(double x, double y, double z, double yaw, double pitch, double head_yaw){
	X = x;
	Y = y;
	Z = z;
	Yaw = yaw;
	Pitch = pitch;
	HeadYaw = head_yaw;
	
	mode = LOCATION_MODE_DIRECT;
	targetX = x;
	targetY = y;
	targetZ = z;
	targetYaw = yaw;
	targetPitch = pitch;
	targetHeadYaw = head_yaw;
	t = ((float)geGetTick()) / 1000.0;
	dt = 0.0;
}

Location::~Location(){
}

void Location::setMode(int m){
	mode = m;
}

int Location::getMode(){
	return mode;
}

void Location::Update(){
	float t2 = ((float)geGetTick()) / 1000.0;
	dt = t2 - t;
	if(mode == LOCATION_MODE_SMOOTH){
		X += (targetX - X) * dt * 10.0;
		Y += (targetY - Y) * dt * 10.0;
		Z += (targetZ - Z) * dt * 10.0;
		Yaw += (targetYaw - Yaw) * dt * 10.0;
		Pitch += (targetPitch - Pitch) * dt * 10.0;
		HeadYaw += (targetHeadYaw - HeadYaw) * dt * 10.0;
	}
	t = t2;
}

float Location::getDeltaT(){
	return dt;
}

double Location::getX(){
	return X;
}

double Location::getY(){
	return Y;
}

double Location::getZ(){
	return Z;
}

void Location::setX(double x){
	if(mode == LOCATION_MODE_SMOOTH){
		targetX = x;
	}else{
		X = x;
	}
}

void Location::setY(double y){
	if(mode == LOCATION_MODE_SMOOTH){
		targetY = y;
	}else{
		Y = y;
	}
}

void Location::setZ(double z){
	if(mode == LOCATION_MODE_SMOOTH){
		targetZ = z;
	}else{
		Z = z;
	}
}

void Location::setXYZ(double x, double y, double z){
	if(mode == LOCATION_MODE_SMOOTH){
		targetX = x;
		targetY = y;
		targetZ = z;
	}else{
		X = x;
		Y = y;
		Z = z;
	}
}

void Location::relativeX(double x){
	if(mode == LOCATION_MODE_SMOOTH){
		targetX += x;
	}else{
		X += x;
	}
}

void Location::relativeY(double y){
	if(mode == LOCATION_MODE_SMOOTH){
		targetY += y;
	}else{
		Y += y;
	}
}

void Location::relativeZ(double z){
	if(mode == LOCATION_MODE_SMOOTH){
		targetZ += z;
	}else{
		Z += z;
	}
}

void Location::relativeXYZ(double x, double y, double z){
	if(mode == LOCATION_MODE_SMOOTH){
		targetX += x;
		targetY += y;
		targetZ += z;
	}else{
		X += x;
		Y += y;
		Z += z;
	}
}

double Location::getYaw(){
	return Yaw;
}

double Location::getPitch(){
	return Pitch;
}

double Location::getHeadYaw(){
	return HeadYaw;
}

void Location::setYaw(double yaw){
	if(mode == LOCATION_MODE_SMOOTH){
		targetYaw = yaw;
	}else{
		Yaw = yaw;
	}
}

void Location::setPitch(double pitch){
	if(mode == LOCATION_MODE_SMOOTH){
		targetPitch = pitch;
	}else{
		Pitch = pitch;
	}
}

void Location::setHeadYaw(double yaw){
	if(mode == LOCATION_MODE_SMOOTH){
		targetHeadYaw = yaw;
	}else{
		HeadYaw = yaw;
	}
}

bool Location::operator== (Location& loc){
	if(loc.X == X && loc.Y == Y && loc.Z == Z && loc.Pitch == Pitch && loc.Yaw == Yaw){
		return true;
	}
	return false;
}

bool Location::operator!= (Location& loc){
	if(loc.X == X && loc.Y == Y && loc.Z == Z && loc.Pitch == Pitch && loc.Yaw == Yaw){
		return false;
	}
	return true;
}
/*
void Location::operator= (Location& loc){
	mode = loc.mode;
	t = loc.t;
	X = loc.X;
	Y = loc.Y;
	Z = loc.Z;
	Yaw = loc.Yaw;
	Pitch = loc.Pitch;
	HeadYaw = loc.HeadYaw;
	targetX = loc.targetX;
	targetY = loc.targetY;
	targetZ = loc.targetZ;
	targetYaw = loc.targetYaw;
	targetPitch = loc.targetPitch;
	targetHeadYaw = loc.targetHeadYaw;
}
*/